package com.gitee.wsl.mathematics.geometry.d2.contour.util

import com.gitee.wsl.ext.number.approx
import com.gitee.wsl.mathematics.function.Equations
import com.gitee.wsl.mathematics.function.Scalars
import com.gitee.wsl.mathematics.geometry.Interval
import com.gitee.wsl.mathematics.geometry.d2.contour.box.Box
import com.gitee.wsl.mathematics.geometry.d2.contour.box.Box2
import com.gitee.wsl.mathematics.geometry.d2.contour.util.Intersections.PARAMETRIC_EPSILON
import com.gitee.wsl.mathematics.geometry.d2.curve.CubicBezier2
import com.gitee.wsl.mathematics.geometry.d2.curve.CurveShape2d
import com.gitee.wsl.mathematics.geometry.d2.curve.CurveLine
import com.gitee.wsl.mathematics.geometry.d2.curve.QuadraticBezier2
import com.gitee.wsl.mathematics.geometry.d2.curve.signum
import com.gitee.wsl.mathematics.vector.Vector
import com.gitee.wsl.mathematics.vector.ext.approx
import com.gitee.wsl.mathematics.vector.ext.cross
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.ext.lerp
import com.gitee.wsl.mathematics.vector.ext.swap
import com.gitee.wsl.mathematics.vector.vec2.Vec2f
import kotlin.jvm.JvmName
import kotlin.math.abs
import kotlin.math.ceil
import kotlin.math.floor
import kotlin.math.max
import kotlin.math.min

//交点

@JvmName("kartifexHackSort1")
fun <T> Array<T>.sort(start: Int = 0, end: Int = size, selector: (T) -> Double) {
    if (start != 0 || end != size) {
        val copy = copyOfRange(start, end)
        copy.sortBy(selector)
        copy.copyInto(this, start)
    } else {
        sortBy(selector)
    }
}

@JvmName("kartifexHackSort2")
fun <T> sort(a: Array<T>, start: Int = 0, end: Int = a.size, selector: (T) -> Double) {
    a.sort(start, end, selector)
}


internal fun<T:Number,V:Vector<T, V>> V.mapRound():V =
    create( proxy.map { Intersections.round(it.toDouble(), PARAMETRIC_EPSILON) + zero })

object Intersections {
    // utilities
    const val FAT_LINE_PARAMETRIC_RESOLUTION = 1e-5
    const val FAT_LINE_SPATIAL_EPSILON = 1e-5

    const val PARAMETRIC_EPSILON = 1e-5
    const val SPATIAL_EPSILON = 1e-5

    const val MAX_CUBIC_CUBIC_INTERSECTIONS = 9

    val PARAMETRIC_BOUNDS: Box2 =
        Box.box(Vec2f(0.0, 0.0), Vec2f(1.0, 1.0))

    fun lineCurve(a: CurveLine, b: CurveShape2d): Array<Vec2f> {
        return if (b is CurveLine) {
            lineLine(a, b)
        } else if (b.isFlat(SPATIAL_EPSILON)) {
            lineLine(a, CurveLine.line(b.start(), b.end()))
        } else if (b is QuadraticBezier2) {
            lineQuadratic(a, b)
        } else {
            lineCubic(a, b as CubicBezier2)
        }
    }

    fun lineLine(a: CurveLine, b: CurveLine): Array<Vec2f> {
        val av: Vec2f = a.end()-a.start()
        val bv: Vec2f = b.end()-b.start()
        val d = av cross bv
        if (abs(d) < 1e-6) {
            val ints: Array<Vec2f> = collinearIntersection(a, b)
            if (ints.all { v: Vec2f ->
                    a.position(v.x.toDouble()) approx b.position(v.y.toDouble())
                   /* Vec.equals(
                        a.position(v.x),
                        b.position(v.y),
                        SPATIAL_EPSILON
                    )*/
                }
            ) {
                return ints
            } else if (abs(d) == 0.0f) {
                return emptyArray()
            }
        }
        val asb = a.start()-b.start()
        val s = (bv cross asb) / d
        val t = (av cross asb) / d
        return arrayOf(Vec2f(s, t))
    }

    fun lineQuadratic(
        p: CurveLine,
        q: QuadraticBezier2
    ): Array<Vec2f> {
        // (p0 - 2p1 + p2) t^2 + (-2p0 + 2p1) t + p0
        val a: Vec2f = q.p0+(q.p1*(-2.0))+(q.p2)
        val b: Vec2f = q.p0*(-2.0)+(q.p1*(2.0))
        val c: Vec2f = q.p0
        val dir: Vec2f = p.end()-p.start()
        val n = Vec2f(-dir.y, dir.x)
        val roots = Equations.solveQuadratic(
            (n dot a), (n dot b), (n dot c) + (p.start() cross p.end())
        )
        val result: Array<Vec2f> = if (Scalars.equals(dir.x.toDouble(), 0.0, Scalars.EPSILON)) {
            val y0 = p.start().y
            Array(roots.size) { i ->
                val t = roots[i]
                val y1 = q.position(t.toDouble()).y
                Vec2f((y1 - y0) / dir.y, t)
            }
        } else {
            val x0 = p.start().x
            Array(roots.size) { i ->
                val t = roots[i]
                val x1 = q.position(t.toDouble()).x
                Vec2f((x1 - x0) / dir.x, t)
            }
        }
        return result
    }

    fun lineCubic(
        p: CurveLine,
        q: CubicBezier2
    ): Array<Vec2f> {
        // (-p0 + 3p1 - 3p2 + p3) t^3 + (3p0 - 6p1 + 3p2) t^2 + (-3p0 + 3p1) t + p0
        val a: Vec2f = q.p0*(-1.0)+(q.p1*(3.0))+(q.p2*(-3.0))+(q.p3)
        val b: Vec2f = q.p0*(3.0)+(q.p1*(-6.0))+(q.p2*(3.0))
        val c: Vec2f = q.p0*(-3.0)+(q.p1*(3.0))
        val d: Vec2f = q.p0
        val dir: Vec2f = p.end()-(p.start())
        val dLen = dir.length
        val n = Vec2f(-dir.y, dir.x)
        val roots = Equations.solveCubic(
            (n dot a),
            (n dot b),
            (n dot c),
            (n dot d) + (p.start() cross p.end())
        )
        val result = Array(roots.size) { i ->
            val t = roots[i]
            val v = q.position(t.toDouble())-(p.start())
            val vLen = v.length
            val s = vLen / dLen * signum((dir dot v).toDouble())
            Vec2f(s, t)
        }
        return result
    }

    fun normalize(intersections: Array<Vec2f>): Array<Vec2f> {
        var limit = intersections.size
        if (limit == 0) {
            return intersections
        }
        var readIdx: Int
        var writeIdx: Int

        // round and filter within [0, 1]
        readIdx = 0
        writeIdx = 0
        while (readIdx < limit) {
            val i: Vec2f = intersections[readIdx].mapRound()
            if (PARAMETRIC_BOUNDS.contains(i)) {
                intersections[writeIdx++] = i
            }
            readIdx++
        }
        limit = writeIdx
        if (limit > 1) {
            // dedupe intersections on b
            //intersections.sortBy { it.y }
            sort(
                intersections,
                0,
                limit,
                { v: Vec2f -> v.y.toDouble() }
            )
            readIdx = 0
            writeIdx = -1
            while (readIdx < limit) {
                val i: Vec2f = intersections[readIdx]
                if (writeIdx < 0 || !(intersections[writeIdx].y approx i.y)) {
                    intersections[++writeIdx] = i
                }
                readIdx++
            }
            limit = writeIdx + 1
        }
        if (limit > 1) {
            // dedupe intersections on a
            sort(
                intersections,
                0,
                limit,
                { v: Vec2f -> v.x.toDouble() })

            readIdx = 0
            writeIdx = -1
            while (readIdx < limit) {
                val i: Vec2f = intersections[readIdx]
                if (writeIdx < 0 || !(intersections[writeIdx].x approx  i.x)) {
                    intersections[++writeIdx] = i
                }
                readIdx++
            }
            limit = writeIdx + 1
        }
        val result: Array<Vec2f> = Array(limit) {
            intersections[it]
        }
        return result
    }

    // analytical methods
    fun collinearIntersection(
        a: CurveShape2d,
        b: CurveShape2d
    ): Array<Vec2f> {
        val result = mutableListOf<Vec2f>()
        for (i in 0..1) {
            val tb: Double = b.nearestPoint(a.position(i.toDouble()))

            // a overhangs the start of b
            if (tb <= 0) {
                val s: Double = round(
                    a.nearestPoint(b.start()),
                    PARAMETRIC_EPSILON
                )
                if (s in 0.0..1.0) {
                    result+(Vec2f(s, 0.0))
                }

                // a overhangs the end of b
            } else if (tb >= 1) {
                val s: Double = round(
                    a.nearestPoint(b.end()),
                    PARAMETRIC_EPSILON
                )
                if (s in 0.0..1.0) {
                    result+(Vec2f(s, 1.0))
                }

                // a is contained in b
            } else {
                result+(Vec2f(i.toDouble(), tb))
            }
        }
        if (result.size == 2 && (result[0] approx result[1])) {
            result.removeLast()
        }
        return result.toTypedArray()
    }

    // subdivision (slow, but as close to a reference implementation as exists)
    class CurveInterval(
        curve: CurveShape2d,
        tLo: Double,
        tHi: Double,
        pLo: Vec2f,
        pHi: Vec2f
    ) {
        val curve: CurveShape2d
        val isFlat: Boolean
        val tLo: Double
        val tHi: Double
        val pLo: Vec2f
        val pHi: Vec2f
        fun bounds(): Box2 {
            return Box.box(pLo, pHi)
        }

        fun intersects(c: CurveInterval): Boolean {
            return bounds().expand(SPATIAL_EPSILON).intersects(c.bounds())
        }

        fun split(): Array<CurveInterval> {
            return if (isFlat) {
                arrayOf(this)
            } else {
                val tMid = (tLo + tHi) / 2
                val pMid: Vec2f = curve.position(tMid)
                arrayOf(
                    CurveInterval(curve, tLo, tMid, pLo, pMid),
                    CurveInterval(curve, tMid, tHi, pMid, pHi)
                )
            }
        }

        fun intersections(c: CurveInterval, acc: MutableList<Vec2f>) {
            for (i in lineLine(CurveLine.line(pLo, pHi), CurveLine.line(c.pLo, c.pHi))) {
                if (PARAMETRIC_BOUNDS.expand(PARAMETRIC_EPSILON).contains(i)) {
                    acc+(lerp(Vec2f(tLo, c.tLo), Vec2f(tHi, c.tHi), i))
                }
            }
        }

        override fun toString(): String {
            return "[$tLo, $tHi]"
        }

        companion object {
            fun from(c: CurveShape2d): Array<CurveInterval?> {
                val ts: DoubleArray = c.inflections()
                ts.sort()
                return if (ts.isEmpty()) {
                    arrayOf(CurveInterval(c, 0.0, 1.0, c.start(), c.end()))
                } else {
                    val ls = arrayOfNulls<CurveInterval>(ts.size + 1)
                    for (i in ls.indices) {
                        val lo: Double = if (i == 0) 0.0 else ts[i - 1]
                        val hi: Double = if (i == ls.size - 1) 1.0 else ts[i]
                        ls[i] = CurveInterval(c, lo, hi, c.position(lo), c.position(hi))
                    }
                    ls
                }
            }
        }

        init {
            this.curve = curve
            this.tLo = tLo
            this.tHi = tHi
            this.pLo = pLo
            this.pHi = pHi
            isFlat = ((pLo approx  pHi) || tHi - tLo < PARAMETRIC_EPSILON || curve.range(tLo, tHi).isFlat(SPATIAL_EPSILON))
        }
    }


    // post-processing
    fun round(n: Double, epsilon: Double): Double {
        return if (Scalars.equals(n, 0.0, epsilon)) {
            0.0
        } else if (Scalars.equals(n, 1.0, epsilon)) {
            1.0
        } else {
            n
        }
    }



    //
    fun intersections(a: CurveShape2d, b: CurveShape2d): Array<Vec2f> {
        if (!a.bounds().expand(SPATIAL_EPSILON).intersects(b.bounds())) {
            return emptyArray()
        }
        return if (a is CurveLine) {
            normalize(lineCurve(a, b))
        } else if (b is CurveLine) {
            val result: Array<Vec2f> = normalize(
                lineCurve(b, a)
            )
            for (i in result.indices) {
                result[i] = result[i].swap()
            }
            result
        } else {
            //return subdivisionCurveCurve(a, b);
            fatLineCurveCurve(a, b)
        }
    }

    // fatline

    // fat lines (faster, but more temperamental)
    // This is adapted from Sederberg's "Curve Intersection Using Bezier Clipping", but the algorithm as described
    // gets unstable when one curve is clipped small enough, causing it to over-clip the other curve, causing us to miss
    // intersection points.  To address this, we quantize the curve sub-ranges using FAT_LINE_PARAMETRIC_RESOLUTION,
    // preventing them from getting too small, and expand the width of our clipping regions by FAT_LINE_SPATIAL_EPSILON.
    fun signedDistance(p: Vec2f, a: Vec2f, b: Vec2f): Float {
        val d: Vec2f = b-(a)
        return ((p cross d) + (b cross a)) / d.length
    }

    fun fatLineWidth(c: CurveShape2d): Interval {
        return when (c) {
            is CurveLine -> {
                Interval.interval(0.0, 0.0)
            }

            is QuadraticBezier2 -> {
                val b: QuadraticBezier2 = c
                Interval.interval(0.0f, signedDistance(b.p1, b.p0, b.p2) / 2)
            }

            is CubicBezier2 -> {
                val b: CubicBezier2 = c
                val d1 = signedDistance(b.p1, b.p0, b.p3)
                val d2 = signedDistance(b.p2, b.p0, b.p3)
                val k = if (d1 * d2 < 0) 4 / 9.0 else 3 / 4.0
                Interval.interval(
                    min(0.0f, min(d1, d2)) * k,
                    max(0.0f, max(d1, d2)) * k
                )
            }

            else -> {
                throw IllegalStateException()
            }
        }
    }

    fun convexHull(
        a: Vec2f,
        b: Vec2f,
        c: QuadraticBezier2
    ): Array<Vec2f> {
        val p0 = Vec2f(0.0, signedDistance(c.p0, a, b))
        val p1 = Vec2f(1 / 2.0, signedDistance(c.p1, a, b))
        val p2 = Vec2f(1.0, signedDistance(c.p2, a, b))
        return arrayOf(p0, p1, p2, p0)
    }

    fun convexHull(
        a: Vec2f,
        b: Vec2f,
        c: CubicBezier2
    ): Array<Vec2f> {
        val p0 = Vec2f(0.0, signedDistance(c.p0, a, b))
        val p1 = Vec2f(1 / 3.0, signedDistance(c.p1, a, b))
        val p2 = Vec2f(2 / 3.0, signedDistance(c.p2, a, b))
        val p3 = Vec2f(1.0, signedDistance(c.p3, a, b))
        val d1 = signedDistance(p1, p0, p3)
        val d2 = signedDistance(p2, p0, p3)
        return if (d1 * d2 < 0) {
            arrayOf(p0, p1, p3, p2, p0)
        } else {
            val k = d1 / d2
            if (k >= 2) {
                arrayOf(p0, p1, p3, p0)
            } else if (k <= 0.5) {
                arrayOf(p0, p2, p3, p0)
            } else {
                arrayOf(p0, p1, p2, p3, p0)
            }
        }
    }

    fun convexHull(
        a: Vec2f,
        b: Vec2f,
        c: CurveShape2d
    ): Array<Vec2f> {
        return when (c) {
            is QuadraticBezier2 -> {
                convexHull(a, b, c)
            }
            is CubicBezier2 -> {
                convexHull(a, b, c)
            }
            else -> {
                throw IllegalStateException()
            }
        }
    }

    fun clipHull(fatLine: Interval, hull: Array<Vec2f>): Interval {
        var lo = Float.POSITIVE_INFINITY
        var hi = Float.NEGATIVE_INFINITY
        for (i in 0 until hull.size - 1) {
            if (fatLine.contains(hull[i].y.toDouble())) {
                lo = min(lo, hull[i].x)
                hi = max(hi, hull[i].x)
            }
        }
        for (y in doubleArrayOf(fatLine.lo, fatLine.hi)) {
            for (i in 0 until hull.size - 1) {
                val a: Vec2f = hull[i]
                val b: Vec2f = hull[i + 1]
                if (Interval.interval(a.y, b.y).contains(y)) {
                    if (a.y == b.y) {
                        lo = min(lo, min(a.x, b.x))
                        hi = max(lo, max(a.x, b.x))
                    } else {
                        val t = Scalars.lerp(a.x.toDouble(), b.x.toDouble(), (y - a.y) / (b.y - a.y))
                        lo = min(lo, t.toFloat())
                        hi = max(hi, t.toFloat())
                    }
                }
            }
        }
        return if (hi < lo) {
            Interval.EMPTY
        } else {
            Interval.interval(lo, hi)
        }
    }

    fun quantize(t: Interval): Interval {
        val resolution: Double = FAT_LINE_PARAMETRIC_RESOLUTION
        val lo: Double = min(
            1 - resolution,
            floor(t.lo / resolution) * resolution
        )
        val hi: Double = max(
            lo + resolution,
            ceil(t.hi / resolution) * resolution
        )
        return Interval.interval(lo, hi)
    }

    fun addIntersections(
        a: FatLine,
        b: FatLine,
        acc: MutableList<Vec2f>
    ) {
        val la: CurveLine = a.line()
        val lb: CurveLine = b.line()
        val av: Vec2f = la.end()-(la.start())
        val bv: Vec2f = lb.end()-(lb.start())
        val asb: Vec2f = la.start()-(lb.start())
        val d = (av cross bv)
        val i = Vec2f(
            (bv cross asb) / d,
            (av cross asb) / d
        )
        if (PARAMETRIC_BOUNDS.expand(0.1f).contains(i)) {
            acc+(Box.box(a.t, b.t).lerp(i))
        }
    }

    fun clipFatline(
        subject: FatLine,
        clipper: FatLine
    ): FatLine? {
        val hull = convexHull(clipper.range.start(), clipper.range.end(), subject.range)

        val expanded = clipper._line.expand(FAT_LINE_SPATIAL_EPSILON)
        val normalized = clipHull(expanded, hull)
        return if (normalized.isEmpty) null else FatLine(
            subject.curve,
            subject.t.lerp(normalized)
        )
    }

    class FatLine internal constructor(curve: CurveShape2d, t: Interval) {
        val curve: CurveShape2d
        val range: CurveShape2d
        val t: Interval
        val _line: Interval
        fun mid(): Double {
            return t.lerp(0.5)
        }

        val isFlat: Boolean
            get() = t.size() < PARAMETRIC_EPSILON || _line.size() <= SPATIAL_EPSILON

        fun bounds(): Box2 {
            return Box.box(range.start(), range.end())
        }

        fun intersects(l: FatLine): Boolean {
            return bounds().expand(SPATIAL_EPSILON * 10).intersects(l.bounds())
        }

        fun split(): Array<FatLine> {
            return if (isFlat) {
                arrayOf(this)
            } else arrayOf(
                FatLine(curve, Interval.interval(t.lo, mid())),
                FatLine(curve, Interval.interval(mid(), t.hi))
            )
        }

        fun line(): CurveLine {
            return CurveLine.line(range.start(), range.end())
        }

        override fun toString(): String {
            return "FatLine(curve=$curve, range=$range, t=$t, _line=$_line, isFlat=$isFlat)"
        }


        companion object {
            fun from(c: CurveShape2d): Array<FatLine> {
                val ts: DoubleArray = c.inflections()
                ts.sort()
                return if (ts.isEmpty()) {
                    arrayOf(FatLine(c, Interval.interval(0.0, 1.0)))
                } else {
                    val result = arrayOfNulls<FatLine>(ts.size + 1)
                    for (i in result.indices) {
                        val lo: Double = if (i == 0) 0.0 else ts[i - 1]
                        val hi: Double = if (i == result.size - 1) 1.0 else ts[i]
                        result[i] = FatLine(c, Interval.interval(lo, hi))
                    }
                    @Suppress("UNCHECKED_CAST")
                    result as Array<FatLine>
                }
            }
        }

        init {
            this.curve = curve
            this.t = quantize(t)
            range = curve.range(this.t)
            _line = fatLineWidth(range)
        }
    }

//    fun fatLineWidth(c: CurveShape2d): Interval? {
//        return if (c is Line2) {
//            Interval.interval(0.0, 0.0)
//        } else if (c is Bezier2.QuadraticBezier2) {
//            val b: Bezier2.QuadraticBezier2 = c as Bezier2.QuadraticBezier2
//            Interval.interval(
//                0.0,
//                Intersections.signedDistance(b.p1, b.p0, b.p2) / 2
//            )
//        } else if (c is Bezier2.CubicBezier2) {
//            val b: Bezier2.CubicBezier2 = c as Bezier2.CubicBezier2
//            val d1: Double = Intersections.signedDistance(b.p1, b.p0, b.p3)
//            val d2: Double = Intersections.signedDistance(b.p2, b.p0, b.p3)
//            val k = if (d1 * d2 < 0) 4 / 9.0 else 3 / 4.0
//            Interval.interval(
//                min(
//                    0.0,
//                    min(d1, d2)
//                ) * k,
//                max(0.0, max(d1, d2)) * k
//            )
//        } else {
//            throw IllegalStateException()
//        }
//    }

//    fun quantize(t: Interval): Interval? {
//        val resolution: Double = Intersections.FAT_LINE_PARAMETRIC_RESOLUTION
//        val lo: Double = min(
//            1 - resolution,
//            floor(t.lo / resolution) * resolution
//        )
//        val hi: Double = max(
//            lo + resolution,
//            ceil(t.hi / resolution) * resolution
//        )
//        return Interval.interval(lo, hi)
//    }

    fun fatLineCurveCurve(a: CurveShape2d, b: CurveShape2d): Array<Vec2f> {
        val queue = ArrayDeque<FatLine>()
        val `as` = FatLine.from(a)
        val bs = FatLine.from(b)
        for (ap in `as`) {
            for (bp in bs) {
                queue.apply {
                    addLast(ap)
                    addLast(bp)
                }
            }
        }
        var iterations = 0
        var collinearCheck = false
        val acc = ArrayDeque<Vec2f>()
        while (queue.size > 0) {
            // if it's taking a while, check once (and only once) if they're collinear
            if (iterations > 32 && !collinearCheck) {
                collinearCheck = true
                val `is`: Array<Vec2f> =
                    collinearIntersection(a, b)
                if (isCollinear(a, b, `is`)) {
                    return `is`
                }
            }
            var lb = queue.removeLast()
            var la = queue.removeLast()
            while (true) {
                iterations++
                if (!la.intersects(lb)) {
                    break
                }
                if (la.isFlat && lb.isFlat) {
                    addIntersections(la, lb, acc)
                    break
                }
                val aSize: Double = la.t.size()
                val bSize: Double = lb.t.size()

                // use a to clip b
                val lbPrime = clipFatline(lb, la) ?: break
                lb = lbPrime

                // use b to clip a
                val laPrime: FatLine = clipFatline(la, lb) ?: break
                la = laPrime
                val ka: Double = la.t.size() / aSize
                val kb: Double = lb.t.size() / bSize
                if (max(ka, kb) > 0.8) {
                    // TODO: switch over to subdivision at some point?
                    for (ap in la.split()) {
                        for (bp in lb.split()) {
                            queue.apply {
                                addLast(ap)
                                addLast(bp)
                            }
                        }
                    }
                    break
                }
            }
        }
        return normalize(acc.toTypedArray())
    }

    private fun isCollinear(
        a: CurveShape2d,
        b: CurveShape2d,
        `is`: Array<Vec2f>
    ): Boolean {
        if (`is`.size != 2) {
            return false
        }
        for (i in 0 until MAX_CUBIC_CUBIC_INTERSECTIONS + 1) {
            val t: Double = i.toDouble() / MAX_CUBIC_CUBIC_INTERSECTIONS
            val pa: Vec2f = a.position(Scalars.lerp(`is`[0].x.toDouble(), `is`[1].x.toDouble(), t))
            val pb: Vec2f = b.position(Scalars.lerp(`is`[0].y.toDouble(), `is`[1].y.toDouble(), t))
            if (!(pa approx pb)) {
                return false
            }
        }
        return true
    }
}
